#include "ompl/base/goals/GoalState.h"
#include "ompl/base/goals/GoalSampleableRegion.h"
#include "CUDAPoissonRRT.h"
#include "CUDAPoissonRRTKernels.h"
#include "CPUUtility.h"
#include "PoissonSampler.h"
#include "PoissonState.h"
#include "gPlannerCollisionWrapper.h"
#include "GPUMemoryManager.h"
#include "cuda_defs.h"
#include "PlanningParams.h"
#include <cuda_runtime_api.h>
#include "cuda_errorcheck.h"

//#define DEBUG_STEP_VALUES

using namespace ompl;

namespace poisson
{

CUDAPoissonRRT::CUDAPoissonRRT(const ompl::base::SpaceInformationPtr &si, const ompl::app::GeometrySpecification* geometrySpecification)
: GPUPoissonRRT(si, geometrySpecification), resetRequired_(false), 
stateValidityCheckingResolutionR3_(0.01f), stateValidityCheckingResolutionSO3_(0.01f),
numPrecomputedPDSamples_(0), numSamples_(0)
{
	d_sampleData_.samples = NULL;
	d_sampleData_.isTreeNode = NULL;
	d_sampleData_.levels = NULL;
}

CUDAPoissonRRT::~CUDAPoissonRRT()
{
	freeMemory();
}

ompl::base::PlannerStatus CUDAPoissonRRT::solve(const ompl::base::PlannerTerminationCondition &ptc)
{
	ompl::time::point timeStart = time::now();

	resetRequired_ = true;

	SerializedStates h_StartAndGoalStates[2];

	// set start state
	const base::State *st = pis_.nextStart();
	if (st == NULL)
	{
		OMPL_ERROR("There are no valid initial states!");
		return base::PlannerStatus::INVALID_START;
	}
	ConvertStateToFloatVector(si_, st, &(h_StartAndGoalStates[0].x[0]));

	// set goal state
	base::Goal                 *goal   = pdef_->getGoal().get();
	base::GoalState			   *goalState = dynamic_cast<base::GoalState*>(goal);
	ConvertStateToFloatVector(si_, goalState->getState(), &(h_StartAndGoalStates[1].x[0]));
	/*
	h_StartAndGoalStates[0].components.q[0] = 0.0;
	h_StartAndGoalStates[0].components.q[1] = 0.0;
	h_StartAndGoalStates[0].components.q[2] = 0.0;
	h_StartAndGoalStates[0].components.q[3] = 1.0;
	h_StartAndGoalStates[1].components.q[0] = 0.0;
	h_StartAndGoalStates[1].components.q[1] = 0.0;
	h_StartAndGoalStates[1].components.q[2] = 0.0;
	h_StartAndGoalStates[1].components.q[3] = 1.0;
	*/

	poisson::cuda::setStartAndGoalStates(h_StartAndGoalStates, numPrecomputedPDSamples_, d_sampleData_, numSamples_, d_startTree_, d_goalTree_);

	// test
	/*
	base::State *tempState1 = si_->allocState();
	base::State *tempState2 = si_->allocState();
	std::vector<double> from(7);
	std::vector<double> to(7);
	base::State *tstate = si_->allocState();
	std::pair<base::State*,double> lastValid(tstate, 0.0);
	from[0] = 270;
	from[1] = 160;
	from[2] = -400;
	from[3] = 0;
	from[4] = 0;
	from[5] = 0;
	from[6] = 1;
	to[0] = 231.407;
	to[1] = 173.931;
	to[2] = -409.597;
	to[3] = 0.075;
	to[4] = 0.730;
	to[5] = -0.549;
	to[6] = sqrt(1 - to[3] * to[3] - to[4] * to[4] - to[5] * to[5]);
	si_->getStateSpace()->deserialize(tempState1, &(from[0]));
	si_->getStateSpace()->deserialize(tempState2, &(to[0]));
	bool noCollision = si_->checkMotion(tempState1, tempState2, lastValid);
	*/

	// test
	/*
	int numRef = 513;
	int numQueries = 2;
	int numK = 4;
	GpuMemoryManager::MemoryChunk* testDataChunk = GpuMemoryManager::getInstance()->getChunk(sizeof(float) * (numQueries * (numRef)));
	GpuMemoryManager::MemoryChunk* distancesFromIntersectChunk = GpuMemoryManager::getInstance()->getChunk(sizeof(float) * numQueries * (numK + 512));
	GpuMemoryManager::MemoryChunk* sampleIndicesChunk = GpuMemoryManager::getInstance()->getChunk(sizeof(int) * numQueries * (numK + 512));
	float* internalDistances = (float*)testDataChunk->p;
	float* distancesFromIntersect = (float*)distancesFromIntersectChunk->p;
	int* sampleIndices = (int*)sampleIndicesChunk->p;
	GPUMEMSET(distancesFromIntersect, 0, sizeof(float) * numQueries * (numK + 512));
	GPUMEMSET(sampleIndices, 0, sizeof(int) * numQueries * (numK + 512));
	float h_data[2 * 513];
	for (int i = 0; i < 513; ++i)
	{
		h_data[0 * 513 + i] = i + 1000;
		h_data[1 * 513 + i] = 512 - i;
	}
	TOGPU(testDataChunk->p, h_data, sizeof(float) * 513 * numQueries);
	poisson::cuda::knn(NULL, numRef, NULL, numQueries, numK, internalDistances, distancesFromIntersect, sampleIndices);
	float h_result[2 * 4];
	int h_resultIndices[2 * 4];
	FROMGPU(h_result, distancesFromIntersect, sizeof(float) * numQueries * 4);
	FROMGPU(h_resultIndices, sampleIndices, sizeof(int) * numQueries * 4);


	GpuMemoryManager::getInstance()->releaseChunk(testDataChunk);
	GpuMemoryManager::getInstance()->releaseChunk(distancesFromIntersectChunk);
	GpuMemoryManager::getInstance()->releaseChunk(sampleIndicesChunk);
	*/

	bool terminate = false;
	bool expandStartTree = true;
	std::vector<SerializedStates> solution;
	int iterations = 0;
	do
	{
		RrtTree& currentTree = expandStartTree ? d_startTree_ : d_goalTree_;
		RrtTree& otherTree = expandStartTree ? d_goalTree_ : d_startTree_;

#ifdef DEBUG_STEP_VALUES
		float randSample[7];
		int nodeIndex;
		float nearestNode[7];
		float neighborSample[7];
		float randSample2[7];
		int colResult;
		int level[2];
		float nearestNode2[7];
		float neighborSample2[7];
		int colResult2;
		int level2[2];
#endif

		// current tree
		////////////////////////////////////////////////////////////////////////
		// generate random configurations
		GpuMemoryManager::MemoryChunk* destStatesChunk = GpuMemoryManager::getInstance()->getChunk(sizeof(SerializedStates) * NUM_THREADS);
		generateRandomStates((SerializedStates*)destStatesChunk->p);
#ifdef DEBUG_STEP_VALUES
		cudaMemcpy(randSample, destStatesChunk->p, sizeof(float) * 7, cudaMemcpyDeviceToHost);
#endif

		// compute nearest nodes
		GpuMemoryManager::MemoryChunk* distancesChunk = GpuMemoryManager::getInstance()->getChunk(sizeof(float) * NUM_THREADS * (1 + 512));
		GpuMemoryManager::MemoryChunk* nodeIndicesChunk = GpuMemoryManager::getInstance()->getChunk(sizeof(int) * NUM_THREADS * (1 + 512));
		computeNearestNodes(currentTree, (SerializedStates*)destStatesChunk->p, (float*)distancesChunk->p, (int*)nodeIndicesChunk->p);
		GpuMemoryManager::getInstance()->releaseChunk(distancesChunk);
#ifdef DEBUG_STEP_VALUES
		cudaMemcpy(&nodeIndex, nodeIndicesChunk->p, sizeof(int), cudaMemcpyDeviceToHost);
		cudaMemcpy(nearestNode, currentTree.treeNodes + nodeIndex, sizeof(float) * 7, cudaMemcpyDeviceToHost);
		cudaMemcpy(&nodeIndex, currentTree.treeNodeIndices + nodeIndex, sizeof(int), cudaMemcpyDeviceToHost);
		cudaMemcpy(&level[0], d_sampleData_.levels + nodeIndex, sizeof(int), cudaMemcpyDeviceToHost);
#endif

		// compute neighbor sample
		GpuMemoryManager::MemoryChunk* sampleIndicesChunk = GpuMemoryManager::getInstance()->getChunk(sizeof(int) * NUM_THREADS);
		computeNeighborSample(currentTree, (int*)nodeIndicesChunk->p, (SerializedStates*)destStatesChunk->p, (int*)sampleIndicesChunk->p);
		GpuMemoryManager::getInstance()->releaseChunk(destStatesChunk);
#ifdef DEBUG_STEP_VALUES
		cudaMemcpy(&nodeIndex, sampleIndicesChunk->p, sizeof(int), cudaMemcpyDeviceToHost);
		cudaMemcpy(neighborSample, d_sampleData_.samples + nodeIndex, sizeof(float) * 7, cudaMemcpyDeviceToHost);
		cudaMemcpy(&level[1], d_sampleData_.levels + nodeIndex, sizeof(int), cudaMemcpyDeviceToHost);
#endif

		// collision check
		GpuMemoryManager::MemoryChunk* resultsChunk = GpuMemoryManager::getInstance()->getChunk(sizeof(int) * NUM_THREADS);
		GpuMemoryManager::MemoryChunk* colFreeStatesChunk = GpuMemoryManager::getInstance()->getChunk(sizeof(SerializedStates) * NUM_THREADS);
		checkCollision(currentTree, (int*)nodeIndicesChunk->p, (int*)sampleIndicesChunk->p, (int*)resultsChunk->p, (SerializedStates*)colFreeStatesChunk->p);
#ifdef DEBUG_STEP_VALUES
		cudaMemcpy(&colResult, resultsChunk->p, sizeof(int), cudaMemcpyDeviceToHost);
#endif

		// remove duplicated nodeId-sampleId pair
		removeDuplicatedPairs((int*)nodeIndicesChunk->p, (int*)sampleIndicesChunk->p, (int*)resultsChunk->p);

		// add new nodes
		addNewNodes(currentTree, otherTree, (int*)nodeIndicesChunk->p, (int*)sampleIndicesChunk->p, (int*)resultsChunk->p, (SerializedStates*)colFreeStatesChunk->p);
		GpuMemoryManager::getInstance()->releaseChunk(nodeIndicesChunk);

		// other tree
		////////////////////////////////////////////////////////////////////////

		// generate configurations for other tree
		destStatesChunk = GpuMemoryManager::getInstance()->getChunk(sizeof(SerializedStates) * NUM_THREADS);
		generateDestStatesForOtherTree((int*)sampleIndicesChunk->p, (int*)resultsChunk->p, (SerializedStates*)colFreeStatesChunk->p,
			(SerializedStates*)destStatesChunk->p);
		GpuMemoryManager::getInstance()->releaseChunk(sampleIndicesChunk);
		GpuMemoryManager::getInstance()->releaseChunk(resultsChunk);
		GpuMemoryManager::getInstance()->releaseChunk(colFreeStatesChunk);
#ifdef DEBUG_STEP_VALUES
		cudaMemcpy(randSample2, destStatesChunk->p, sizeof(float) * 7, cudaMemcpyDeviceToHost);
#endif

		// compute nearest nodes
		distancesChunk = GpuMemoryManager::getInstance()->getChunk(sizeof(float) * NUM_THREADS);
		nodeIndicesChunk = GpuMemoryManager::getInstance()->getChunk(sizeof(int) * NUM_THREADS);
		computeNearestNodes(otherTree, (SerializedStates*)destStatesChunk->p, (float*)distancesChunk->p, (int*)nodeIndicesChunk->p);
		GpuMemoryManager::getInstance()->releaseChunk(distancesChunk);
#ifdef DEBUG_STEP_VALUES
		cudaMemcpy(&nodeIndex, nodeIndicesChunk->p, sizeof(int), cudaMemcpyDeviceToHost);
		cudaMemcpy(nearestNode2, otherTree.treeNodes + nodeIndex, sizeof(float) * 7, cudaMemcpyDeviceToHost);
		cudaMemcpy(&nodeIndex, otherTree.treeNodeIndices + nodeIndex, sizeof(int), cudaMemcpyDeviceToHost);
		cudaMemcpy(&level2[0], d_sampleData_.levels + nodeIndex, sizeof(int), cudaMemcpyDeviceToHost);
#endif

		// compute neighbor sample
		sampleIndicesChunk = GpuMemoryManager::getInstance()->getChunk(sizeof(int) * NUM_THREADS);
		computeNeighborSample(otherTree, (int*)nodeIndicesChunk->p, (SerializedStates*)destStatesChunk->p, (int*)sampleIndicesChunk->p);
		GpuMemoryManager::getInstance()->releaseChunk(destStatesChunk);
#ifdef DEBUG_STEP_VALUES
		cudaMemcpy(&nodeIndex, sampleIndicesChunk->p, sizeof(int), cudaMemcpyDeviceToHost);
		cudaMemcpy(neighborSample2, d_sampleData_.samples + nodeIndex, sizeof(float) * 7, cudaMemcpyDeviceToHost);
		cudaMemcpy(&level2[1], d_sampleData_.levels + nodeIndex, sizeof(int), cudaMemcpyDeviceToHost);
#endif

		// collision check
		resultsChunk = GpuMemoryManager::getInstance()->getChunk(sizeof(int) * NUM_THREADS);
		colFreeStatesChunk = GpuMemoryManager::getInstance()->getChunk(sizeof(SerializedStates) * NUM_THREADS);
		checkCollision(otherTree, (int*)nodeIndicesChunk->p, (int*)sampleIndicesChunk->p, (int*)resultsChunk->p, (SerializedStates*)colFreeStatesChunk->p);
#ifdef DEBUG_STEP_VALUES
		cudaMemcpy(&colResult2, resultsChunk->p, sizeof(int), cudaMemcpyDeviceToHost);
#endif

		// remove duplicated nodeId-sampleId pair
		removeDuplicatedPairs((int*)nodeIndicesChunk->p, (int*)sampleIndicesChunk->p, (int*)resultsChunk->p);

		// add new nodes
		addNewNodes(otherTree, currentTree, (int*)nodeIndicesChunk->p, (int*)sampleIndicesChunk->p, (int*)resultsChunk->p, (SerializedStates*)colFreeStatesChunk->p);
		GpuMemoryManager::getInstance()->releaseChunk(nodeIndicesChunk);
		GpuMemoryManager::getInstance()->releaseChunk(sampleIndicesChunk);
		GpuMemoryManager::getInstance()->releaseChunk(resultsChunk);
		GpuMemoryManager::getInstance()->releaseChunk(colFreeStatesChunk);

#ifdef DEBUG_STEP_VALUES
		printf("(%3.2f,%3.2f,%3.2f) %d (%3.2f,%3.2f,%3.2f|%3.2f,%3.2f,%3.2f,%3.2f)Lv.%d\n",
			randSample[0], randSample[1], randSample[2], expandStartTree ? 1 : 2,
			nearestNode[0], nearestNode[1], nearestNode[2], nearestNode[3], nearestNode[4], nearestNode[5], nearestNode[6], level[0]);
		printf("(%3.2f,%3.2f,%3.2f) %d (%3.2f,%3.2f,%3.2f|%3.2f,%3.2f,%3.2f,%3.2f)Lv.%d %s\n",
			randSample[0], randSample[1], randSample[2], expandStartTree ? 1 : 2,
			neighborSample[0], neighborSample[1], neighborSample[2], neighborSample[3], neighborSample[4], neighborSample[5], neighborSample[6], level[1],
			(colResult == 0) ? "O" : "X");

		printf("(%3.2f,%3.2f,%3.2f) %d (%3.2f,%3.2f,%3.2f|%3.2f,%3.2f,%3.2f,%3.2f)Lv.%d\n",
			randSample2[0], randSample2[1], randSample2[2], expandStartTree ? 2 : 1,
			nearestNode2[0], nearestNode2[1], nearestNode2[2], nearestNode2[3], nearestNode2[4], nearestNode2[5], nearestNode2[6], level2[0]);
		printf("(%3.2f,%3.2f,%3.2f) %d (%3.2f,%3.2f,%3.2f|%3.2f,%3.2f,%3.2f,%3.2f)Lv.%d %s\n\n",
			randSample2[0], randSample2[1], randSample2[2], expandStartTree ? 2 : 1,
			neighborSample2[0], neighborSample2[1], neighborSample2[2], neighborSample2[3], neighborSample2[4], neighborSample2[5], neighborSample2[6], level2[1],
			(colResult2 == 0) ? "O" : "X");
#endif


		// termination check
		terminate = isSolutionFound(solution);

		expandStartTree = !expandStartTree;
		++iterations;

	} while (!terminate && !ptc);

	double planningTime = time::seconds(time::now() - timeStart);
	printf("%d iterations after %f sec\n", iterations, planningTime);
	if (!ptc)
	{
		for (int i = 0; i < solution.size(); ++i)
		{
			float euler[3];
			float qx = solution[i].components.q[0];
			float qy = solution[i].components.q[1];
			float qz = solution[i].components.q[2];
			float qw = solution[i].components.q[3];
			euler[0] = atan2f(2*(qx*qw+qy*qz),1-2*(qx*qx+qy*qy));
			euler[1] = asinf(2*(qw*qy-qx*qz));
			euler[2] = atan2f(2*(qz*qw+qx*qy),1-2*(qy*qy+qz*qz));
			euler[0] *= 180.f / 3.141592f;
			euler[1] *= 180.f / 3.141592f;
			euler[2] *= 180.f / 3.141592f;

			float dist = 0;
			if (i > 0)
			{
				dist = (solution[i].x[0]-solution[i-1].x[0]) * (solution[i].x[0]-solution[i-1].x[0]) +
					(solution[i].x[1]-solution[i-1].x[1]) * (solution[i].x[1]-solution[i-1].x[1]) +
					(solution[i].x[2]-solution[i-1].x[2]) * (solution[i].x[2]-solution[i-1].x[2]);
				dist = sqrtf(dist);
			}

			printf("%d (%.3f %.3f %.3f | %.3f %.3f %.3f) adv:%f\n",
				i,
				solution[i].x[0],
				solution[i].x[1],
				solution[i].x[2],
				euler[0], euler[1], euler[2],
				dist);
		}
		printf("\n");
		printf("# of precomputed samples : %d\n", numPrecomputedPDSamples_);
		printf("# of samples from adaptive sampling: %d\n", *d_sampleData_.h_numSamples - numPrecomputedPDSamples_);
		printf("# of nodes in start tree : %d\n", *d_startTree_.h_numTreeNodes);
		printf("# of nodes in goal tree : %d\n", *d_goalTree_.h_numTreeNodes);
	}

	bool solved = false;
	bool approximate = false;
	return base::PlannerStatus(solved, approximate);
}

void CUDAPoissonRRT::clear(void)
{
	GPUPoissonRRT::clear();

	if (resetRequired_)
		reset();
}

void CUDAPoissonRRT::reset()
{
	// TODO:
	// set numSmples as numPrecomputedSamples
	// reset trees
	numSamples_ = numPrecomputedPDSamples_;
	*d_sampleData_.h_numSamples = numSamples_;
	TOGPU(d_sampleData_.numSamples, d_sampleData_.h_numSamples, sizeof(int));

	poisson::cuda::reset();

	resetRequired_ = false;
}

void CUDAPoissonRRT::setup(void)
{
	GPUPoissonRRT::setup();

	PoissonSampler* poissonSampler = dynamic_cast<PoissonSampler*>(sampler_.get());

	// bounds
	std::vector<float> vBoundsMin(3);
	std::vector<float> vBoundsMax(3);
	for (int i = 0; i < 3; ++i)
	{
		vBoundsMin[i] = (float)poissonSampler->GetBoundsMin()[i];
		vBoundsMax[i] = (float)poissonSampler->GetBoundsMax()[i];
	}

	// state validity checking resolution
	base::StateSpacePtr spaceR3 = si_->getStateSpace()->as<base::CompoundStateSpace>()->getSubspace(0);
	base::StateSpacePtr spaceSO3 = si_->getStateSpace()->as<base::CompoundStateSpace>()->getSubspace(1);
	stateValidityCheckingResolutionR3_ = (float)((double)spaceR3->getValidSegmentCountFactor()
		/ (spaceR3->getMaximumExtent() * spaceR3->getLongestValidSegmentFraction()));
	stateValidityCheckingResolutionSO3_ = (float)((double)spaceSO3->getValidSegmentCountFactor()
		/ (spaceSO3->getMaximumExtent() * spaceSO3->getLongestValidSegmentFraction()));

	// precomputed poisson states
	const std::vector<PoissonState*>& poissonStates = poissonSampler->GetPoissonStates();
	numPrecomputedPDSamples_ = poissonStates.size();
	numSamples_ = numPrecomputedPDSamples_;

	double vSerialized[7];
	std::vector<SerializedStates> vSerializedStates(numPrecomputedPDSamples_);
	for (int i = 0; i < numPrecomputedPDSamples_; ++i)
	{
		si_->getStateSpace()->serialize(vSerialized, poissonStates[i]->getState());
		for (int j = 0; j < 7; ++j)
			vSerializedStates[i].x[j] = (float)vSerialized[j];
	}

	// precomputed adaptive template
	const std::vector<PoissonState*>& templateStates = poissonSampler->GetTemplateStates();
	std::vector<SerializedStates> vSerializedTemplateStates(templateStates.size());
	for (int i = 0; i < templateStates.size(); ++i)
	{
		si_->getStateSpace()->serialize(vSerialized, templateStates[i]->getState());
		for (int j = 0; j < 7; ++j)
			vSerializedTemplateStates[i].x[j] = (float)vSerialized[j];
	}

	// meshes
	GMesh* h_obstacle;
	GMesh* h_robot;
	gplanner_collision_wrapper::setup(geometrySpecification_, &h_obstacle, &h_robot);

	// one-time memory allocation
	initializeMemory();

	// upload
	uploadData(vBoundsMin, vBoundsMax, h_robot, h_obstacle, vSerializedStates, vSerializedTemplateStates);

	// initialize random states
	poisson::cuda::initializeRandomStates();

	GpuMemoryManager::getInstance()->init();
}

void CUDAPoissonRRT::freeMemory(void)
{
	GPUPoissonRRT::freeMemory();

	poisson::cuda::freeMemory(d_sampleData_, d_templateData_, d_robot_, d_obstacle_, d_startTree_, d_goalTree_);
	
	GpuMemoryManager::getInstance()->destroy();
}

void CUDAPoissonRRT::initializeMemory()
{
	poisson::cuda::initializeMemory(d_sampleData_, &d_robot_, &d_obstacle_, d_startTree_, d_goalTree_);
}

void CUDAPoissonRRT::uploadData(const std::vector<float>& vBoundsMin, const std::vector<float>& vBoundsMax, 
	const GMesh* h_robot, const GMesh* h_obstacle, 
	const std::vector<SerializedStates>& vSerializedStates, const std::vector<SerializedStates>& vTemplateStates)
{
	poisson::cuda::uploadData(vBoundsMin, vBoundsMax, h_robot, h_obstacle, vSerializedStates, vTemplateStates, numPrecomputedPDSamples_,
		d_robot_, d_obstacle_, d_sampleData_, d_templateData_);
}

// primitive methods
////////////////////////////////////////////////////////////////////////////////
void CUDAPoissonRRT::generateRandomStates(SerializedStates* outStates)
{
	poisson::cuda::generateRandomStates(outStates);
}

void CUDAPoissonRRT::computeNearestNodes(RrtTree& tree, SerializedStates* inStates, float* outDistances, int* outNodeIndices)
{
	int numTreeNodes = (int)(*(tree.h_numTreeNodes));
	GpuMemoryManager::MemoryChunk* internalDistancesChunk = GpuMemoryManager::getInstance()->getChunk(sizeof(float) * NUM_THREADS * numTreeNodes);
	float* internalDistances = (float*)internalDistancesChunk->p;

	poisson::cuda::knn(tree.treeNodes, numTreeNodes, inStates, NUM_THREADS, 1, internalDistances, outDistances, outNodeIndices);

	GpuMemoryManager::getInstance()->releaseChunk(internalDistancesChunk);
}

void CUDAPoissonRRT::computeNeighborSample(RrtTree& tree, int* inNodeIndices, SerializedStates* inDestStates, int* outSampleIndices)
{
	// get distances from tree nodes to random states
	GpuMemoryManager::MemoryChunk* internalDistR3Chunk = GpuMemoryManager::getInstance()->getChunk(sizeof(float) * NUM_THREADS);
	GpuMemoryManager::MemoryChunk* internalDistSO3Chunk = GpuMemoryManager::getInstance()->getChunk(sizeof(float) * NUM_THREADS);
	float* distR3 = (float*)internalDistR3Chunk->p;
	float* distSO3 = (float*)internalDistSO3Chunk->p;
	poisson::cuda::computeDistancesFromTreeNodesToStates(NUM_THREADS, tree.treeNodes, inNodeIndices, inDestStates, distR3, distSO3);

	// compute intersection states
	GpuMemoryManager::MemoryChunk* internalStatesChunk = GpuMemoryManager::getInstance()->getChunk(sizeof(SerializedStates) * NUM_THREADS);
	SerializedStates* intersectionStates = (SerializedStates*)internalStatesChunk->p;
	poisson::cuda::computeIntersectionStates(NUM_THREADS, tree.treeNodes, inNodeIndices, tree.treeNodeIndices, d_sampleData_.levels,
		inDestStates, distR3, distSO3, intersectionStates);
	GpuMemoryManager::getInstance()->releaseChunk(internalDistR3Chunk);
	GpuMemoryManager::getInstance()->releaseChunk(internalDistSO3Chunk);

	// get knn poisson samples near intersection states in the disk radius
	int numSamples = (*(d_sampleData_.h_numSamples));
	unsigned int k = __min(MAX_NUM_NEAREST_NEIGHBOR, numSamples);
	GpuMemoryManager::MemoryChunk* internalDistancesChunk = GpuMemoryManager::getInstance()->getChunk(sizeof(float) * NUM_THREADS * (numSamples));
	GpuMemoryManager::MemoryChunk* distancesFromIntersectChunk = GpuMemoryManager::getInstance()->getChunk(sizeof(float) * NUM_THREADS * (k + 512));
	GpuMemoryManager::MemoryChunk* sampleIndicesChunk = GpuMemoryManager::getInstance()->getChunk(sizeof(int) * NUM_THREADS * (k + 512));
	float* internalDistances = (float*)internalDistancesChunk->p;
	float* distancesFromIntersect = (float*)distancesFromIntersectChunk->p;
	int* sampleIndices = (int*)sampleIndicesChunk->p;
	poisson::cuda::knn(d_sampleData_.samples, numSamples, intersectionStates, NUM_THREADS, k, internalDistances, distancesFromIntersect, sampleIndices);
	GpuMemoryManager::getInstance()->releaseChunk(internalDistancesChunk);

	// compute distances to random states
	GpuMemoryManager::MemoryChunk* distancesToDestChunk = GpuMemoryManager::getInstance()->getChunk(sizeof(float) * NUM_THREADS * k);
	float* distancesToDest = (float*)distancesToDestChunk->p;
	poisson::cuda::computeDistancesToDestStates(NUM_THREADS, k, d_sampleData_.samples, sampleIndices, d_sampleData_.levels,
		distancesFromIntersect, inDestStates, distancesToDest, tree.treeNodeIndices, inNodeIndices);
	GpuMemoryManager::getInstance()->releaseChunk(distancesFromIntersectChunk);

	// get the closest(nearest neighbor) samples to random states
	GpuMemoryManager::MemoryChunk* minDistancesChunk = GpuMemoryManager::getInstance()->getChunk(sizeof(float) * NUM_THREADS * k);
	float* minDistances = (float*)minDistancesChunk->p;
	poisson::cuda::computeMinDistanceSamples(NUM_THREADS, k, d_sampleData_.samples, sampleIndices, distancesToDest, outSampleIndices, minDistances);
	GpuMemoryManager::getInstance()->releaseChunk(sampleIndicesChunk);
	GpuMemoryManager::getInstance()->releaseChunk(distancesToDestChunk);

	// generate new samples at intersection states if no samples in the disk
	poisson::cuda::generateNewSamplesIfNoNeighbor(NUM_THREADS, d_sampleData_, minDistances, intersectionStates, outSampleIndices,
		tree.treeNodeIndices, inNodeIndices, inDestStates);
	GpuMemoryManager::getInstance()->releaseChunk(internalStatesChunk);
	GpuMemoryManager::getInstance()->releaseChunk(minDistancesChunk);
}

void CUDAPoissonRRT::checkCollision(RrtTree& currentTree, int* inNodeIndices, int* inDestStateIndices, int* outResults, SerializedStates* outColFreeStates)
{
	// out: outResults has collision result (0: collision-free, 1: collision detected)
	//      outColFreeStates has last collision-free states
	poisson::cuda::checkCollision(NUM_THREADS, currentTree.treeNodes, inNodeIndices, d_sampleData_.samples, inDestStateIndices,
		outResults, outColFreeStates,
		d_robot_, d_obstacle_, stateValidityCheckingResolutionR3_, stateValidityCheckingResolutionSO3_);
}

void CUDAPoissonRRT::removeDuplicatedPairs(int* inNodeIndices, int* inDestStateIndices, int* inResults)
{
	// if a pair has the same node index, same sample index, change result to IN_COLLISION_WITH_NO_ADVANCE to avoid adding duplicated new samples
	poisson::cuda::removeDuplicatedPairs(NUM_THREADS, inNodeIndices, inDestStateIndices, inResults);
}

void CUDAPoissonRRT::addNewNodes(RrtTree& currentTree, RrtTree& otherTree, int* inNodeIndices, int* inDestStateIndices,
	int* inResults, SerializedStates* inColFreeStates)
{
	// if no collision
	//   connect
	// if collision 
	//   generate new samples using template
	//   shrink sample
	poisson::cuda::addNewNodes(NUM_THREADS, currentTree, otherTree, d_sampleData_, inNodeIndices, inDestStateIndices, inResults, inColFreeStates, d_templateData_);

	// update host values
	poisson::cuda::downloadValues(d_sampleData_, currentTree);
}

void CUDAPoissonRRT::generateDestStatesForOtherTree(int* inDestStateIndices, int* inResults, SerializedStates* inColFreeStates, 
	SerializedStates* outStates)
{
	return poisson::cuda::generateDestStatesForOtherTree(NUM_THREADS, d_sampleData_, inDestStateIndices, inResults, inColFreeStates, outStates);
}

bool CUDAPoissonRRT::isSolutionFound(std::vector<SerializedStates>& solution)
{
	return poisson::cuda::isSolutionFound(d_sampleData_, d_startTree_, d_goalTree_, solution);
}

}
